#pragma once
#include <iostream>
#include <mutex>
#include <thread>
#include <iomanip>
#include <ctime>

#include <opencv2/opencv.hpp>
#include <opencv2/core/cuda.hpp>
#include <opencv2/cudaarithm.hpp>
#include <opencv2/cudawarping.hpp>
#include <opencv2/dnn.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/cudafilters.hpp>

#include <cuda_runtime_api.h>
#include "NvInfer.h"
#include "NvOnnxConfig.h"
#include "NvOnnxParser.h"
#include "NvInferRuntime.h"
#include "NvInferRuntimeCommon.h"
#include "NvInferPlugin.h"
#include "gpu_common.h"

#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>

#include "../common/athena/json.hpp"
#include "../common/athena/logging.h"
#include "../common/tensorrt/argsParser.h"
#include "../common/tensorrt/logging.h"

using namespace nvinfer1;
using namespace nvonnxparser;
using namespace cv;

class Camera_cross_view_base_opencv_gpu
{
    using json = nlohmann::json;

protected:
    // 配置文件路径
    std::string config_file_path_;
    json camera_cross_view_base_config_json;

    // task setup
    bool input_concate_image_mask;
    int input_concate_image_mask_car_name;
    int model_type;
    bool delete_sky;
    double delete_sky_proportion;
    bool post_processing;
    bool erode_dilate;
    bool show_result;

    // model setup
    std::string trtfile;
    IExecutionContext *context;
    map<string, pair<int, size_t>> engine_name_size;
    ICudaEngine *engine;
    cudaStream_t stream;
    void *buffers[2];
    size_t net_input_image_size;
    size_t net_output_tensor_size;
    int net_input_height_;
    int net_input_width_;
    int net_input_channel_;
    int net_input_batch_size_;
    int net_output_height_;
    int net_output_width_;
    int net_output_channel_;
    int net_output_batch_size_;
    int concate_image_count;

    // post_processing setup
    cv::Mat erode_element;
    cv::Mat dilate_element;
    int dilate_shape;
    int erode_shape;
    int dilate_iterations;
    int erode_iterations;
    cv::Point dilate_anchor;
    cv::Point erode_anchor;

    // image setup
    float mean[3];
    float std[3];
    int show_image_resize_height;
    int show_image_resize_width;
    std::vector<std::vector<cv::Point>> input_image_mask_lines;
    double infer_distance_front;
    double infer_distance_back;
    double infer_distance_left;
    double infer_distance_right;
    double input_mask_scale;

    // ros setup
    int advertise_queue_size;
    int subscribe_queue_size;
    double last_frame_timestamp;

    // sub topic
    std::vector<std::string> image_raw_input_topic_vect;                                     // 原始图片输入向量
    std::vector<message_filters::Subscriber<sensor_msgs::Image> *> sub_cam_message_ptr_vect; // 订阅输入图片消息指针向量

    // pub topic
    image_transport::Publisher pub_cross_view_result_image;      // 发布分割结果图片
    image_transport::Publisher pub_cross_view_result_show_image; // 发布分割结果show image图片
    image_transport::Publisher pub_cross_view_input_net_image;   // 发布输入网络图片

    // time analyse
    std::clock_t startTime_0;
    std::clock_t endTime_0;
    std::clock_t total_prepare_startTime_0;
    std::clock_t total_prepare_endTime_0;
    std::clock_t cv_bridge_toCvCopy_startTime_0;
    std::clock_t cv_bridge_toCvCopy_endTime_0;
    std::clock_t model_infer_startTime_0;
    std::clock_t model_infer_endTime_0;
    std::clock_t post_processing_startTime_0;
    std::clock_t post_processing_endTime_0;
    std::clock_t erode_and_dilate_startTime_0;
    std::clock_t erode_and_dilate_endTime_0;
    std::clock_t PrepareImage_startTime_0;
    std::clock_t PrepareImage_endTime_0;
    std::clock_t PrepareImage_resize_startTime_0;
    std::clock_t PrepareImage_resize_endTime_0;
    std::clock_t PrepareImage_concate_resize_startTime_0;
    ;
    std::clock_t PrepareImage_concate_resize_endTime_0;
    std::clock_t PrepareImage_memcpy_startTime_0;
    std::clock_t PrepareImage_memcpy_endTime_0;
    std::clock_t PrepareImage_mask_startTime_0;
    std::clock_t PrepareImage_mask_endTime_0;

public:
    Camera_cross_view_base_opencv_gpu(std::string file_path) : config_file_path_(file_path), it_(ros_nodehandle), ros_nodehandle_private("~"){};
    ~Camera_cross_view_base_opencv_gpu() = default;
    void Init();
    void Join();
    bool Init_Ros();
    void Init_config();
    bool Cross_view_model_init(const std::string &trtfile);
    void Cross_view_inference_function(std::vector<sensor_msgs::ImageConstPtr> image_message_vect);
    void Callback_Cross_view_base(const sensor_msgs::ImageConstPtr &image_message_0);
    void Callback_Cross_view_base(const sensor_msgs::ImageConstPtr &image_message_0,
                                  const sensor_msgs::ImageConstPtr &image_message_1,
                                  const sensor_msgs::ImageConstPtr &image_message_2);
    void Callback_Cross_view_base(const sensor_msgs::ImageConstPtr &image_message_0,
                                  const sensor_msgs::ImageConstPtr &image_message_1,
                                  const sensor_msgs::ImageConstPtr &image_message_2,
                                  const sensor_msgs::ImageConstPtr &image_message_3);
    void Callback_Cross_view_base(const sensor_msgs::ImageConstPtr &image_message_0,
                                  const sensor_msgs::ImageConstPtr &image_message_1,
                                  const sensor_msgs::ImageConstPtr &image_message_2,
                                  const sensor_msgs::ImageConstPtr &image_message_3,
                                  const sensor_msgs::ImageConstPtr &image_message_4,
                                  const sensor_msgs::ImageConstPtr &image_message_5);
    double get_current_frame_image_timestamp(const sensor_msgs::ImageConstPtr &image_message_0);
    void drawCrossMark(cv::Mat dst, cv::Point pt, int model_type);

    void PrepareImage_concate(std::vector<cv::Mat> image_vect, float *gpu_input);
    std::vector<float> PrepareImage_concate(std::vector<cv::Mat> image_vect);
    ros::NodeHandle ros_nodehandle;
    ros::NodeHandle ros_nodehandle_private;
    ros::NodeHandle ros_road_information_contours;
    image_transport::ImageTransport it_;

    ros::Subscriber sub;

    typedef message_filters::sync_policies::ApproximateTime<
        sensor_msgs::Image,
        sensor_msgs::Image,
        sensor_msgs::Image>
        MySyncPolicy_2;

    typedef message_filters::sync_policies::ApproximateTime<
        sensor_msgs::Image,
        sensor_msgs::Image,
        sensor_msgs::Image,
        sensor_msgs::Image>
        MySyncPolicy_3;

    typedef message_filters::sync_policies::ApproximateTime<
        sensor_msgs::Image,
        sensor_msgs::Image,
        sensor_msgs::Image,
        sensor_msgs::Image,
        sensor_msgs::Image,
        sensor_msgs::Image>
        MySyncPolicy_5;
};
